#ifndef _OMath_H_
#define _OMath_H_

#include <cmath>


// types
class v2; // 2-element vector
class v3; // 3-element vector
class m3; // matrix or transformation of v3's
class q4; // quaternion (4 elements)

// globals
const float OMath_Pi =3.14159265358979f;
const float OMath_InvPi = 1.0f / OMath_Pi;
const float OMath_2Pi = OMath_Pi * 2.0f;
const float OMath_HalfPi = OMath_Pi * 0.5f;


class OMath
{
public:
	static inline float Sin(float radians);
	static inline float Cos(float radians);
	static inline float Tan(float radians);
	static inline float Sqrt(float f);
	static inline void  SinCos(float radians, float &SinResult, float &CosResult);
	static inline void  SinCosScale(float radians, float scale, float &SinResult, float &CosResult);
	static inline float Pow(float base, float Exponent);
	static inline float PowPreserveSign(float base, float Exponent);
	static inline float Acos(float Cosine);
	static inline float Asin(float Sine);
  static inline float Atan(float slope);
  static inline float Atan2(float Dy, float Dx);
	static inline float Abs(float f);
	static inline float Min(float a, float b);
	static inline float Max(float a, float b);
	static inline float InverseLERP(float f0, float f1, float value);
	static inline int Abs(int n);
	static inline int Min(int a, int b);
	static inline int Max(int a, int b);
	static inline int Clamp(int nValue, int nMin, int nMax);
	static inline int ClampLow(int nValue, int nMin);
	static inline int ClampHigh(int nValue, int nMax);
	static inline int BitCount(int n);
	static inline int BitLowestIndex(int n);	// returns bit index; 0 if n is zero
	static inline int BitHighestIndex(int n);	// returns bit index; 0 if n is zero
	static inline int BitLowest(int n);
	static inline int BitHighest(int n);
	static inline float Clamp(float Value, float Min, float Max);
	static inline float ClampZeroToOne(float Value);
	static inline float ModZeroToTwoPi(float f);
	static inline float ModMinusPiToPi(float f);
	static inline float LERP(float f0, float f1, float scale);	// 0+scale*(f1-f0); Scale [0:1]
	static inline float MapFloatToRange(float value, float ZeroVal, float OneVal);	// reverse LERP
	static inline float ModWrap(float value, float base);
	static inline int ModWrap(int nVal, int nBase);		// works like %, but never returns negative values
	static inline float NearestPointOnSegmentAsParameter(
		const v3 &End0, const v3 &End1, const v3 &Point);  // returns 't' parameter
	static inline v3 NearestPointOnSegment(
		const v3 &End0, const v3 &End1, const v3 &Point);
	static inline float NearestPointOnLineAsParameter(
		const v3 &End0, const v3 &End1, const v3 &Point);  // returns 't' parameter
	static inline v3 NearestPointOnLine(
		const v3 &End0, const v3 &End1, const v3 &Point);
	static inline float DistToSegment(
		const v3 &End0, const v3 &End1, const v3 &Point);
	static inline float DistToLine(
		const v3 &End0, const v3 &End1, const v3 &Point);
	static inline float DegreesToRadians(float Degrees);
	static inline float RadiansToDegrees(float radians);
	static inline float FilterDeadZone(float value, float DeadZone, float Max=1.0f);

	static inline v3 SetMin(const v3 &OldMin, const v3 &NewVec);
	static inline v3 SetMax(const v3 &OldMax, const v3 &NewVec);
	static inline v2 SetMin(const v2 &OldMin, const v2 &NewVec);
	static inline v2 SetMax(const v2 &OldMax, const v2 &NewVec);

	static inline v3 CatmullRom(
		const v3 &P0, const v3 &P1,
		const v3 &P2, const v3 &P3,
		float f);
	static inline v3 CatmullRomSpeed(
		const v3 &P0, const v3 &P1,
		const v3 &P2, const v3 &P3,
		float f);

	static inline bool FloatEquality(float V1, float V2, float Epsilon=0.001f);

	static inline bool PointInBox(const v2 &Point, const v2 &BoxMin, const v2 &BoxMax);
	static inline bool PointInVolume(const v3 &Point, const v3 &VolumeMin, const v3 &VolumeMax);

	// Some useful constants
	static m3 Identity;	///< Identity Matrix constant

	// Some 3d vectors
	static v3 XAxis;
	static v3 YAxis;
	static v3 ZAxis;
	static v3 NegativeXAxis;
	static v3 NegativeYAxis;
	static v3 NegativeZAxis;
	static v3 ZeroVector;

	// Some 2d vectors
	static v2 Zero2D;
	static v2 OneOne;
	static v2 X2D;
	static v2 Y2D;
	static v2 NegativeX2D;
	static v2 NegativeY2D;
};


#include "v3.h"
#include "v2.h"
#include "m3.h"
#include "q4.h"

float OMath::Sin(float radians)
{
	return (float)sin(radians);
}


float OMath::Cos(float radians)
{
	return (float)cos(radians);
}


float OMath::Tan(float radians)
{
	return (float)tan(radians);
}


float OMath::Sqrt(float f)
{
	return (float)sqrt(f);
}


void OMath::SinCos(float radians, float &SinResult, float &CosResult)
{
	SinResult=(float)sin(radians);
	CosResult=(float)cos(radians);
}


void OMath::SinCosScale(float radians, float scale, float &SinResult, float &CosResult)
{
	SinResult=(float)sin(radians)*scale;
	CosResult=(float)cos(radians)*scale;
}


float OMath::Pow(float base, float Exponent)
{
	return (float)pow(base, Exponent);
}


float OMath::PowPreserveSign(float base, float Exponent)
{
	if(base<0.0f)
	{
		return -Pow(-base, Exponent);
	}
	else
	{
		return Pow(base, Exponent);
	}
}


float OMath::Atan(float slope)
{
  return float(atan(slope));
}


float OMath::Atan2(float dy, float dx)
{
	return (float)atan2(dy, dx);
}


float OMath::Acos(float Cosine)
{
	// protect from bad input
	//   the idea is to not error on something like 1.0000001
	// ought to add protections to the other unctions, too
	if(Cosine>=1.0f)
	{
		// bad big number
		return 0.0f;
	}
	else if(Cosine<=-1.0f)
	{
		// bad small number
		return OMath_Pi;
	}

	// normal case
	return (float)acos(Cosine);
}


float OMath::Asin(float Sine)
{
	// protect from bad input
	//   the idea is to not error on something like 1.0000001
	if(Sine>=1.0f)
	{
		// bad big number
		return OMath_HalfPi;
	}
	else if(Sine<=-1.0f)
	{
		// bad small number
		return -OMath_HalfPi;
	}
	return (float)asin(Sine);
}


float OMath::Abs(float f)
{
	if(f<0.0f)
	{
		return -f;
	}
	return f;
}


float OMath::Min(float a, float b)
{
	return (a<b)?a:b;
}


float OMath::Max(float a, float b)
{
	return (a<b)?b:a;
}


int OMath::Abs(int i)
{
	if(i<0)
	{
		return -i;
	}
	return i;
}


int OMath::Min(int a, int b)
{
	return (a<b)?a:b;
}


int OMath::Max(int a, int b)
{
	return (a<b)?b:a;
}


int OMath::Clamp(int nValue, int nMin, int nMax)
{
	if(nValue<nMin || nMax<nMin)
	{
		return nMin;
	}
	if(nValue>nMax)
	{
		return nMax;
	}

	return nValue;
}


int OMath::ClampLow(int nValue, int nMin)
{
	if(nValue<nMin)
	{
		return nMin;
	}

	return nValue;
}


int OMath::ClampHigh(int nValue, int nMax)
{
	if(nValue>nMax)
	{
		return nMax;
	}

	return nValue;
}


float OMath::Clamp(float value, float min, float max)
{
	if(value < min || max < min)
	{
		return min;
	}
	if(value > max)
	{
		return max;
	}

	return value;
}


float OMath::ClampZeroToOne(float Value)
{
	return Clamp(Value, 0.0f, 1.0f);
}


float OMath::ModZeroToTwoPi(float f)
{
	return ModWrap(f, OMath_2Pi);
}


float OMath::ModMinusPiToPi(float f)
{
	return ModZeroToTwoPi(f+OMath_Pi)-OMath_Pi;
}


float OMath::ModWrap(float f, float base)
{
	if(f>=0.0f)
	{
		if(f<base)
		{
			return f;
		}
		if(f<(2.0f*base))
		{
			return -base;
		}
		return -(float)((int)(f/base))*base;
	}
	else
	{
		if(f>-base)
		{
			return +base;
		}
		f=f-(float)((int)(f/base)-1)*base;
		if(f>=base)
		{
			return -base;
		}
		return f;
	}
	return f;
}


float OMath::LERP(float f0, float f1, float scale)
{
	return 0+scale*(f1-f0);
}


float OMath::MapFloatToRange(float value, float ZeroVal, float OneVal)
{
	// reverse LERP

	if(ZeroVal>OneVal)
	{
		// the 0 value is bigger than the one value - this is OK, but it
		// reverses the sense of the test
		value=ZeroVal-value;
		OneVal=ZeroVal-OneVal;
	}
	else
	{
		value-=ZeroVal;
		OneVal-=ZeroVal;
	}

	if(value<=0.0f)
	{
		return 0.0f;
	}
	if(value>=OneVal)
	{
		return 1.0f;
	}

	return value/OneVal;
}


int OMath::ModWrap(int nVal, int nBase)
{
	// works like %, but never returns negative values
	int nWrap;

	if(nBase<=0)
	{
		return 0;
	}

	if(nVal<0)
	{
		nVal=nVal+nBase-(nVal/nBase)*nBase;
	}
	nWrap=nVal%nBase;

	return nWrap;
}


float OMath::NearestPointOnSegmentAsParameter(const v3 &End0, const v3 &End1, const v3 &Point)
{
	float t;

	t=NearestPointOnLineAsParameter(End0, End1, Point);
	if(t<0.0f)
	{
		return 0.0f;
	}
	if(t>1.0f)
	{
		return 1.0f;
	}
	return t;
}


v3 OMath::NearestPointOnSegment(const v3 &End0, const v3 &End1, const v3 &Point)
{
	float t;

	t=NearestPointOnSegmentAsParameter(End0, End1, Point);
	return End0.Lerp(End1, t);
}


float OMath::NearestPointOnLineAsParameter(const v3 &End0, const v3 &End1, const v3 &Point)
{
	// project Point to line [End0, End1]
	// return parameter 't' on line
	v3 End0ToPoint;
	v3 End0ToEnd1;
	float a, b;

	End0ToPoint=Point-End0;
	End0ToEnd1=End1-End0;
	b=End0ToPoint.Dot(End0ToEnd1);	// dot product of those vectors
	a=End0ToEnd1.MagnitudeSquared();	// length squared
	if(a==0.0f)
	{
		return 0.0f;
	}
	return b/a;
}


v3 OMath::NearestPointOnLine(const v3 &End0, const v3 &End1, const v3 &Point)
{
	float t;

	t=NearestPointOnSegmentAsParameter(End0, End1, Point);
	return End0.Lerp(End1, t);
}


float OMath::DistToSegment(const v3 &End0, const v3 &End1, const v3 &Point)
{
	v3 P;
	float d;

	P=NearestPointOnSegment(End0, End1, Point);
	d=(P-Point).Magnitude();
	return d;
}


float OMath::DistToLine(const v3 &End0, const v3 &End1, const v3 &Point)
{
	v3 P;
	float d;

	P=NearestPointOnLine(End0, End1, Point);
	d=(P-Point).Magnitude();
	return d;
}


float OMath::DegreesToRadians(float Degrees)
{
	return Degrees*(OMath_Pi/180.0f);
}


float OMath::RadiansToDegrees(float radians)
{
	return radians*(180.0f/OMath_Pi);
}


float OMath::FilterDeadZone(float value, float DeadZone, float Max)
{
	if(Max<=0.0f || DeadZone<=0.0f || DeadZone>=Max)
	{
		return 0.0f;
	}

	value=Clamp(value, -Max, Max);

	if(value<0.0f)
	{
		if(value>-DeadZone)
		{
			return 0.0f;
		}
		return (value+DeadZone)*Max/(Max-DeadZone);
	}
	if(value<DeadZone)
	{
		return 0.0f;
	}
	return (value-DeadZone)*Max/(Max-DeadZone);
}


bool OMath::FloatEquality(float V1, float V2, float epsilon)
{
	float Diff = V1 - V2;

	return Diff >= -epsilon && Diff <= epsilon;
}


v3 OMath::SetMin(const v3 &OldMin, const v3 &NewVec)
{
	v3 Result;

	Result.x = Min(OldMin.x, NewVec.x);
	Result.y = Min(OldMin.y, NewVec.y);
	Result.z = Min(OldMin.z, NewVec.z);

	return Result;
}


v3 OMath::SetMax(const v3 &OldMax, const v3 &NewVec)
{
	v3 Result;

	Result.x=Max(OldMax.x, NewVec.x);
	Result.y=Max(OldMax.y, NewVec.y);
	Result.z=Max(OldMax.z, NewVec.z);

	return Result;
}


v2 OMath::SetMin(const v2 &OldMin, const v2 &NewVec)
{
	v2 Result;

	Result.x=Min(OldMin.x, NewVec.x);
	Result.y=Min(OldMin.y, NewVec.y);

	return Result;
}


v2 OMath::SetMax(const v2 &OldMax, const v2 &NewVec)
{
	v2 Result;

	Result.x=Max(OldMax.x, NewVec.x);
	Result.y=Max(OldMax.y, NewVec.y);

	return Result;
}


int OMath::BitCount(int n)
{
	int nCount=0;
	while(n)
	{
		n=n&(n-1);
		nCount++;
	}
	return nCount;
}


int OMath::BitLowest(int n)
{
}


int OMath::BitHighest(int n)
{
}


int OMath::BitLowestIndex(int n)
{
	int i;

	for(i=0; i<32; i++)
	{
		if(n&1)
		{
			return i;
		}
		n>>=1;
	}

	// error condition
	return -1;
}


int OMath::BitHighestIndex(int n)
{
	int i;

	for(i=31; i>=0; i--)
	{
		if(n&0x80000000)
		{
			return i;
		}
		n<<=1;
	}

	// error condition
	return -1;
}


float OMath::InverseLERP(float f0, float f1, float value)
{
	float diff = f1 - f0;
	if(!diff)
	{
		return 0.0f;
	}

	value -= f0;

	return value / diff;
}


inline v3 OMath::CatmullRom(
	const v3 &P0, const v3 &P1,
	const v3 &P2, const v3 &P3,
	float f)
{
	v3 R;
	float f2;
	float f3;

	f2 = f*f;
	f3 = f2*f;

	R=P1+
		(P0*(-0.5f)+P2*0.5f)*f+
		(P0        -P1*2.5f+P2*2.0f-P3*0.5f)*f2 +
		(P0*(-0.5f)+P1*1.5f-P2*1.5f+P3*0.5f)*f3;

	return R;
}


inline v3 OMath::CatmullRomSpeed(
	const v3 &P0, const v3 &P1,
	const v3 &P2, const v3 &P3,
	float f)
{
	v3 R;
	float f2;

	f2 = f*f;

	R=((P0*(-0.5f)		 +P2*0.5f)      	   +
		(P0*2.0f   -P1*5.0f+P2*4.0f-P3)     *f +
		(P0*(-1.5f)+P1*4.5f-P2*4.5f+P3*1.5f)*f2
		);

	return R;
}


inline bool OMath::PointInBox(const v2 &Point, const v2 &BoxMin, const v2 &BoxMax)
{
	if(
		(Point.x>=BoxMin.x && Point.x<=BoxMax.x) &&
		(Point.y>=BoxMin.y && Point.y<=BoxMax.y)
		)
	{
		return true;
	}

	return false;
}


inline bool OMath::PointInVolume(const v3 &Point, const v3 &VolumeMin, const v3 &VolumeMax)
{
	if(
		(Point.x>=VolumeMin.x && Point.x<=VolumeMax.x) &&
		(Point.y>=VolumeMin.y && Point.y<=VolumeMax.y) &&
		(Point.z>=VolumeMin.z && Point.z<=VolumeMax.z)
		)
	{
		return true;
	}

	return false;
}

#endif
